/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package com.team1160.breakaway;


import com.team1160.breakaway.api.Constants;
import com.team1160.breakaway.autonomous.AutonomousTaskManager;
import com.team1160.breakaway.ballControl.Dribbler;
import com.team1160.breakaway.ballControl.Kicker;
import com.team1160.breakaway.camera.Cam_Drive;
import com.team1160.breakaway.camera.Targetting; 
import com.team1160.breakaway.dashboard.DashBoard;
import com.team1160.breakaway.drive.Drive;
import com.team1160.breakaway.endGame.Arm;
import com.team1160.breakaway.input.AutoInputState;
import com.team1160.breakaway.input.InputReader;
import com.team1160.breakaway.input.InputState;
import com.team1160.breakaway.model.Model;
import com.team1160.breakaway.model.ModelUpdater;
import com.team1160.breakaway.model.RobotState;
import com.team1160.breakaway.model.RobotStateReader;
import com.team1160.breakaway.model.robotCommand.RobotCommand;
import com.team1160.breakaway.model.robotCommand.TeleopTaskManager;
import com.team1160.breakaway.robotOutput.RobotOutput;
import com.team1160.breakaway.util.Pnuematics;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Watchdog;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Team1160 extends IterativeRobot
{

    protected RobotStateReader robotStateReader = RobotStateReader.getInstance();
    protected InputReader inputReader = InputReader.getInstance();
    protected ModelUpdater modelUpdater;
    protected TeleopTaskManager taskManager;
    protected AutonomousTaskManager autoTaskManager;
    protected Targetting targetting;
    protected RobotOutput robotOutput;
    protected DashBoard dash;

    long last = System.currentTimeMillis();

    protected Kicker kicker;
    protected Dribbler dribbler;
    protected Drive drive;
    protected Pnuematics pnuematics;
    protected Cam_Drive camDrive;
    protected Arm arm;

    protected final Model model = Model.getInstance();

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit()
    {
        initRobotComponents();

        initRobotControllers();

        initWatchdog();
    }

    //---------------------------------------------------------------------

    protected void initWatchdog(){
        Watchdog.getInstance().setExpiration(1.5);
    }

    //---------------------------------------------------------------------

    protected void initRobotComponents()
    {
        //camera/targeting thread
        this.targetting = new Targetting();
        this.camDrive   = new Cam_Drive(this.targetting);

        //kicker, dribbler and driver train
        this.kicker     = new Kicker();
        this.drive      = new Drive();    
        this.dribbler   = new Dribbler();
        this.arm = new Arm();
    }

    //---------------------------------------------------------------------

    protected void initRobotControllers()
    {

        //-------------------------
        
        this.modelUpdater = new ModelUpdater(this.model);
        
        this.modelUpdater.addNotifier(this.kicker);
        this.modelUpdater.addNotifier(this.drive);
        //this.modelUpdater.addNotifier(this.dribbler);
        this.modelUpdater.addNotifier(this.camDrive);
        this.modelUpdater.addNotifier(this.arm);

        //update model the first time
        this.modelUpdater.updateModel();

        //-------------------------

        this.taskManager = new TeleopTaskManager();
        this.autoTaskManager = new AutonomousTaskManager();

        this.robotOutput = new RobotOutput(this.kicker, this.dribbler, this.drive, this.camDrive, this.arm);

        this.pnuematics = new Pnuematics();
        
        this.pnuematics.start();

        this.dash = new DashBoard();
    }


    //---------------------------------------------------------------------

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {

    }

    public void autonomousInit(){
        this.autoTaskManager = new AutonomousTaskManager();
    }

    public void autonomousContinuous(){
        RobotState currentRobotState = null;
        AutoInputState autoInputState = null;
        RobotCommand nextRobotCommand = null;

        try{
            currentRobotState = this.robotStateReader.getRobotState();
            autoInputState = this.inputReader.getAutoInputState();
            nextRobotCommand = this.autoTaskManager.getAutonomousRobotCommand(currentRobotState, autoInputState);
            this.robotOutput.implementActions(nextRobotCommand);
            this.modelUpdater.updateModel();
            this.dash.update(currentRobotState);
        }catch(Exception e){
            e.printStackTrace();
        }
    }

    //---------------------------------------------------------------------

    public void teleopPeriodic(){}


    //---------------------------------------------------------------------

    public void disabledPeriodic(){}

    //---------------------------------------------------------------------

    /**
     * This function is called periodically during operator control
     */
    public void teleopInit(){
        //this.targetting.start();
    }

    public void teleopContinuous() {
       

        RobotState currentRobotState = null;
        InputState inputState = null;
        RobotCommand nextRobotCommand = null;

        try{
            //get the current robot state
            currentRobotState = this.robotStateReader.getRobotState();

            //read input state
            inputState = this.inputReader.getInputState();

            //task manager determines the next command
            nextRobotCommand = this.taskManager.getRobotCommand(currentRobotState, inputState);

            this.robotOutput.implementActions(nextRobotCommand);
            this.modelUpdater.updateModel();

            this.dash.update(currentRobotState);
            
        }catch(Exception e){
            e.printStackTrace();
        }

        Watchdog.getInstance().feed();
        /*if(currentRobotState != null) {
            System.out.println("Left Front Speed : " + currentRobotState.getDriveTrainMotorValue(Constants.ID_DRIVETRAIN_LEFT_FRONT));
            System.out.println("Left Back Speed : " + currentRobotState.getDriveTrainMotorValue(Constants.ID_DRIVETRAIN_LEFT_BACK));
            System.out.println("Right Front Speed : " + currentRobotState.getDriveTrainMotorValue(Constants.ID_DRIVETRAIN_RIGHT_FRONT));
            System.out.println("Right Back Speed : " + currentRobotState.getDriveTrainMotorValue(Constants.ID_DRIVETRAIN_RIGHT_BACK));
        }*/
    }

    //---------------------------------------------------------------------
}
